#include <Servo.h> //调用Servo类
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

Servo right;  //创建一个舵机控制对象
Servo left; // 使用Servo类最多可以控制8个舵机
MPU6050 mpu;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];

int pos = 0;    // 该变量用与存储舵机角度位置

void setup() 
{ 
  right.attach(9);  // 该舵机由arduino第九脚控制
  left.attach(8);
  
  Serial.begin(115200);//..............................
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.CalibrateAccel(6);
  mpu.CalibrateGyro(6);
  mpu.PrintActiveOffsets();
  mpu.setDMPEnabled(true);
} 

void loop() 
{ 
  right.write(0);     // 复位
  left.write(0);
    right.write(ypr[2] * 60+150 / M_PI);               // 指定舵机转向的角度
    left.write(ypr[2] * 60+360/ M_PI);
    delay(60);  
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))//............................
  {
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    Serial.print(" \t 摇头");
    Serial.print(ypr[0] * 180 / M_PI);
    Serial.print(" \t 点头");
    Serial.print(ypr[1] * 180 / M_PI);
    Serial.print(" \t 晃头");
    Serial.println(ypr[2] * 180 / M_PI);
  }
}
